#include "WPILib.h"
#include "Robotmap.h"
#include "C:/WindRiver/workspace/SimpleTemplate/Systems/DriveTrain/DriveTrain.h"
#define DEBUG true
/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */
class RobotDemo: public SimpleRobot {
	RobotDrive robot; // robost drive system
	Joystick stick; // only joystick
	DriveTrain driveTrain; //For Self Test Routine
public:
	RobotDemo(void) :
		robot(1, 2), // these must be initialized in the same order
				stick(1) // as they are declared above.
	{
		robot.SetExpiration(0.1);
	}

	void SelfTestIO(void) {
		// Front Left
		driveTrain.SetMotor(FrontLeft, 1.0);
		Wait(1.0);
		driveTrain.SetMotor(FrontLeft, 0.0);
		Wait(1.0);
		driveTrain.SetMotor(FrontLeft, -1.0);
		Wait(1.0);
		driveTrain.SetMotor(FrontLeft, 0.0);
		// Front Right
		driveTrain.SetMotor(FrontRight, 1.0);
		Wait(1.0);
		driveTrain.SetMotor(FrontRight, 0.0);
		Wait(1.0);
		driveTrain.SetMotor(FrontRight, -1.0);
		Wait(1.0);
		driveTrain.SetMotor(FrontRight, 0.0);
		// Back Left
		driveTrain.SetMotor(BackLeft, 1.0);
		Wait(1.0);
		driveTrain.SetMotor(BackLeft, 0.0);
		Wait(1.0);
		driveTrain.SetMotor(BackLeft, -1.0);
		Wait(1.0);
		driveTrain.SetMotor(BackLeft, 0.0);
		// Back Right
		driveTrain.SetMotor(BackRight, 1.0);
		Wait(1.0);
		driveTrain.SetMotor(BackRight, 0.0);
		Wait(1.0);
		driveTrain.SetMotor(BackRight, -1.0);
		Wait(1.0);
		driveTrain.SetMotor(BackRight, 0.0);
		// Solenoids
		// Servos
		// Spikes
		
	}
	/**
	 * Drive left & right motors for 1 seconds then stop
	 */
	void Autonomous(void) {
		if (DEBUG) {
			SelfTestIO();
			return;
		}
		robot.SetSafetyEnabled(false);
		robot.Drive(0.5, 0.0); // drive forwards half speed
		Wait(2.0); //    for 2 seconds
		robot.Drive(0.0, 0.0); // stop robot
	}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void) {
		//myRobot.SetSafetyEnabled(true);
		while (IsOperatorControl()) {

			Wait(0.01);
		}
	}
};
// Code Bunny. GET REKT SCRUB
//				   	    /|      __  
//                     / |   ,-~ /  
//                    Y :|  //  /    
//                    | jj /( .^  
//                    >-"~"-v"  
//                   /       Y    
//                  jo  o    |  
//                 ( ~T~     j   
//                  >._-' _./   
//                 /   "~"  |    
//                Y     _,  |      
//               /| ;-"~ _  l    
//              / l/ ,-"~    \  
//              \//\/      .- \  
//               Y        /    Y*  
//               l       I     ! 
//               ]\      _\    /"\ 
//              (" ~----( ~   Y.  )   
START_ROBOT_CLASS(RobotDemo)
;

